/* Main library functions */

/*
1) Definitions
2) Camera
3) Claw
4) Odometry
5) Music
6) Misc
*/



//////////////// Definitions ////////////////

#define MR 0                     //Right Motor pin#
#define ML 3                     //Left Motor pin#
#define CLAW_HAND 3              //Servo for the claw hand. Range is from 0 (closed) to 255 (open).
#define CLAW_ARM 2               //Servo for the claw arm. Range is from 0 (down) to 140 (up).

#define RR_SENSOR 2              //IR sensor on the back right
#define FR_SENSOR 3              //IR sensor on the front right
#define FL_SENSOR 4              //IR sensor on the front left
#define RL_SENSOR 5              //IR sensor on the back left

#define TOUCH_SENSOR digital(15) //Read the touch sensor on pin#

#define SPEED 100                //Motor speed
#define TURN_SPEED 60            //Motor speed when making a turn

#define pi 3.141592


//////////////// Camera ////////////////
////need to update

/*
#include "xbccamlib.c"

#define BLOB_MIN_SIZE 500        //Minimum blob size in square pixels
#define CENTER 176               //Center x pixel of the camera
#define ERROR_RANGE 70           //Range to the left and right of the center pixel
int last_seen_right = 1;         //Holds last known direction of blob
*/


//////////////// Claw ////////////////

int closeclaw(){                               
    set_servo_position(CLAW_HAND,160);
}
int openclaw(){
    set_servo_position(CLAW_HAND,255);
}
int lowerclaw(){
    set_servo_position(CLAW_ARM,40);
}
int raiseclaw(){
    set_servo_position(CLAW_ARM,128);
}


//////////////// Odometry ////////////////
//// travel(cm)
//// turn(degrees)
//// stop()

#define WHEEL_DIAMETER 5.5
#define TICKS_PER_ROTATION 1100.0
#define WHEEL_BASE 12.9
float WHEEL_CIRCUMFRENCE = pi*WHEEL_DIAMETER;

//Function to travel a given number of centimeters
float travel(float distance){
    float ticks = distance*TICKS_PER_ROTATION/WHEEL_CIRCUMFRENCE;
    printf("Traveling %f cm.\n", distance);
    mrp(MR, SPEED,(long)ticks);
    mrp(ML, SPEED,(long)ticks);
    bmd(MR);
    bmd(ML);
}

//Function to turn a given number of degrees
float turn(float degrees){
    //Ticks = theta*(1100R/360r)
    float ticks = degrees*TICKS_PER_ROTATION*(WHEEL_BASE/2.0)/(360.0*WHEEL_DIAMETER/2.0);
    printf("Turning %f degrees.\n", degrees);
    mrp(ML, TURN_SPEED, -(long)ticks);
    mrp(MR, TURN_SPEED, (long)ticks);
    bmd(MR);
    bmd(ML);
}

//A powered stop
void stop(){
    printf("Stopping.\n");
    freeze(MR);
    freeze(ML);
}

//////////////// Music ////////////////
//// NOTE(octave, duration)

/*
#include "music.c"
*/


//////////////// Misc ////////////////

// Funtion to return a random number between (and including) 0 and max_value
//  Used because srandom() isn't working.
int randint(max_value){
 int a = (random()%(max_value+1));
 return a;
}
